#include "includes.h"
#define GRAMB_ERROR(e, s) { case (e): printf("Gram[b]: %s\n", (s)); break; }

BEGIN_FSM_STATE_TABLE(gramb_state_table)
    BEGIN_STATE(GRAMB_STATE_ERROR, "error", gramb_error, NULL, gramb_error_occur)
        STATE_EVENT_ITEM(NULL, GRAMB_EVENT_RESET, GRAMB_STATE_INIT)
    END_STATE(GRAMB_STATE_ERROR)

    BEGIN_STATE(GRAMB_STATE_INIT, "init", gramb_init, NULL, NULL)
        STATE_EVENT_ITEM(NULL, GRAMB_EVENT_ERROR, GRAMB_STATE_ERROR)
        STATE_EVENT_ITEM(NULL, GRAMB_EVENT_RESET, GRAMB_STATE_INIT)
        STATE_EVENT_ITEM(gramb_start, GRAMB_EVENT_RESUME, GRAMB_STATE_RUNNING)
    END_STATE(GRAMB_STATE_INIT)

    BEGIN_STATE(GRAMB_STATE_STOP, "stop", gramb_stop, gramb_stop_exit, gramb_blink)
        STATE_EVENT_ITEM(NULL, GRAMB_EVENT_ERROR, GRAMB_STATE_ERROR)
        STATE_EVENT_ITEM(NULL, GRAMB_EVENT_RESET, GRAMB_STATE_INIT)
        STATE_EVENT_ITEM(NULL, GRAMB_EVENT_RESUME, GRAMB_STATE_RUNNING)
        STATE_EVENT_ITEM(collect_send, GRAMB_EVENT_SEND, GRAMB_STATE_STOP)
    END_STATE(GRAMB_STATE_STOP)

    BEGIN_STATE(GRAMB_STATE_RUNNING, "running", gramb_resume, NULL, gramb_protect)
        STATE_EVENT_ITEM(NULL, GRAMB_EVENT_ERROR, GRAMB_STATE_ERROR)
        STATE_EVENT_ITEM(NULL, GRAMB_EVENT_RESET, GRAMB_STATE_INIT)
        STATE_EVENT_ITEM(NULL, GRAMB_EVENT_STOP, GRAMB_STATE_STOP)
        STATE_EVENT_ITEM(collect_init, GRAMB_EVENT_COLLECT, GRAMB_STATE_COLLECT)
    END_STATE(GRAMB_STATE_RUNNING)

    BEGIN_STATE(GRAMB_STATE_COLLECT, "collect", NULL, NULL, gramb_collect_countdown)
        STATE_EVENT_ITEM(collect_stop, GRAMB_EVENT_STOP, GRAMB_STATE_STOP)
    END_STATE(GRAMB_STATE_COLLECT)
END_FSM_STATE_TABLE(gramb_state_table)

FSM gramb={GRAMB_STATE_INIT, NULL, gramb_state_table};
FSM_EVENT gramb_event={GRAMB_EVENT_RESUME, 0, 0, 0, 0, 1200, {END_EVENT_ID}};
int gramb_error_code;

extern int cnt[2];
extern track_status gramb_track;
extern led_status led[4];

extern char cmd[2];
extern int cmd_receive;

extern int buffertop;
extern uint16 buffer[BUFFER_SIZE];

void gramb_error(FSM_EVENT *event){
    pit_disable(PIT0);
    pit_disable(PIT1);

    printf("Gram[b]: an error has occurred...\n");

    gpio_set(GPIO_PORTB, 0, 0);
    gpio_set(GPIO_PORTC, 16, 0);
    gpio_set(GPIO_PORTC, 11, 0);
    gpio_set(GPIO_PORTC, 8, 0);

    systick_delay_ms(1000);
}

void gramb_error_occur(FSM_EVENT *event){
    static int cnt;

    ++cnt;
    systick_delay_ms(500);
    gpio_toggle(GPIO_PORTB, 0);
    gpio_toggle(GPIO_PORTC, 16);
    gpio_toggle(GPIO_PORTC, 11);
    gpio_toggle(GPIO_PORTC, 8);

    switch(gramb_error_code){
        GRAMB_ERROR(0x01, "gramb_cmd receive unknown command");
        GRAMB_ERROR(0x02, "UART0 receive 0 character");
    }

    if(cnt==11){
        event_buffer_push(event, GRAMB_EVENT_RESET);
        cnt=0;
    }
}

void gramb_start(FSM_EVENT *event){
    printf("Gram[b]: now starting...\n");

    led[0].val=0;
    led[1].val=1;
    led[2].val=1;
    led[3].val=0;

    dma_adc_activate();

    pit_enable(PIT0);
    pit_enable(PIT1);
}

void gramb_stop(FSM_EVENT *event){
    printf("Gram[b]: now stopping...\n");

    gramb_track.control_lock=1;

    /* stop motor */
    tpm_pwm_duty(TPM0, TPM_CH0, 0);
    tpm_pwm_duty(TPM0, TPM_CH3, 0);

    /* clear anti-pwm */
    tpm_pwm_duty(TPM0, TPM_CH1, 0);
    tpm_pwm_duty(TPM0, TPM_CH2, 0);

    /* light Red LED indicating the car is stopped */
    gpio_set(led[1].port, led[1].ch, 0);
    gpio_set(led[2].port, led[2].ch, 1);
    gpio_set(led[3].port, led[3].ch, 1);

    /* Not switch state until the car is REALLY stopped */
    while(cnt[0]>=2||cnt[1]>=2);

    printf("Gram[b]: now stopped...\n");
}

void gramb_resume(FSM_EVENT *event){
    printf("Gram[b]: now resuming...\n");

    gramb_track.control_lock=0;
    led[3].val=0;
}

void gramb_blink(FSM_EVENT *event){
    systick_delay_ms(500);
    if(!gramb_track.islost){
        printf("unlock!\n");
        event_buffer_push(event, GRAMB_EVENT_RESUME);
    }
}

void gramb_stop_exit(FSM_EVENT *event){
    gpio_set(led[1].port, led[1].ch, 1);
}

void gramb_collect_countdown(FSM_EVENT *event){
    if(event->countdown==0){
        event_buffer_push(event, GRAMB_EVENT_STOP);
        event_buffer_push(event, GRAMB_EVENT_SEND);
    }
}

void gramb_protect(FSM_EVENT *event){
    /* protect gramb from running out of track */
    /*
    if(gramb_track.islost){
        printf("lock!\n");
        event_buffer_push(event, GRAMB_EVENT_STOP);
    }
    */
}

void collect_init(FSM_EVENT *event){
    led[1].val=0;
    led[2].val=1;
    led[3].val=1;
}

void collect_send(FSM_EVENT *event){
    int i,j;
    led[1].val=1;
    led[2].val=1;
    led[3].val=0;
    led_update();
    pit_disable(PIT1);
    printf("send command 's' to receive data collected...\n");
    while(!cmd_receive||cmd[0]!='s');
    led[1].val=1;
    led[2].val=0;
    led[3].val=1;
    led_update();
    cmd_receive=0;
    printf("start sending data...\n--------------------------\nbuffertop=%d\n", buffertop);
    for(i=0; i+100<buffertop; i+=100){
        for(j=0; j<100; j+=4)
            printf("%d#%d#%d#%d#\n", buffer[i+j], buffer[i+j+1], buffer[i+j+2], buffer[i+j+3]);
    }
    for(j=0; i+j<buffertop; j+=4)
        printf("%d#%d#%d#%d#\n", buffer[i+j], buffer[i+j+1], buffer[i+j+2], buffer[i+j+3]);
    for(; i+j<buffertop; ++j)
        printf("%d#", buffer[i+j]);
    printf("\n-------------------------------------------------\ndata end\n");
    led[1].val=1;
    led[2].val=1;
    led[3].val=1;
    led_update();
    pit_enable(PIT1);
}

void collect_stop(FSM_EVENT *event){
}

void gramb_switch(uint16 *x, uint16 t){
    if((x[t]&0x1)!=(x[!t]&0x1)&&(x[t]&0x1)==0x1){
        printf("collect!\n");
        event_buffer_push(&gramb_event, GRAMB_EVENT_COLLECT);
    }
}

int main(void){
    uint16 input[2]={0},t=0;
    systick_delay_ms(1500);
    gramb_init(&gramb_event);
    while(1){
        input[t]=read_input();
        /* input has changed */
        if(input[t]!=input[!t])
            gramb_switch(input, t);
        if(gramb_event.f!=gramb_event.r)
            gramb_event.id=event_buffer_pop(&gramb_event);
        fsm_function(&gramb, &gramb_event);
        t=!t;
    }
}
